#include "mult_lidar/pointcloud_processor.hpp"
#include <rclcpp/rclcpp.hpp> 
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/common/transforms.h>
#include <cmath>

namespace mult_lidar
{
    
    PointCloudProcessor::PointCloudProcessor()
        : frame_counter_(0){
            voxel_filter_=std::make_unique<VoxelVotingFilter>();
        }

    void PointCloudProcessor::setParameters(const ProcessingParams& params){
        params_ = params;

        //更新变换矩阵
        transform_matrix_ = Eigen::Affine3f::Identity();

        float angle_rad = params_.rotation_angle * M_PI / 180.0f;

        Eigen::Matrix3f rotation;

        rotation << cos(angle_rad), 0, sin(angle_rad),
                                0,  1,  0,
                    -sin(angle_rad), 0, cos(angle_rad);
        transform_matrix_.linear() = rotation * transform_matrix_.linear();
        //transform_matrix_.translation() << 0, 0, params_.radar_height;

        //更新体素滤波器参数
        voxel_filter_->setParameters(0.01f, 5, 4);
        
    }

    void PointCloudProcessor::transformToBaseFrame(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud){
        pcl::transformPointCloud(*cloud, *cloud, transform_matrix_);
    }

    void PointCloudProcessor::filterPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud){
        pcl::PassThrough<pcl::PointXYZI> pass;

        //1、移除后方的点 ，x轴为车体前进方向
        pass.setInputCloud(cloud);
        pass.setFilterFieldName("x");
        pass.setFilterLimits(params_.x_min, params_.x_max);
        pass.filter(*cloud);

        //2、限制左右探测距离，y轴方向
        pass.setInputCloud(cloud);
        pass.setFilterFieldName("y");
        pass.setFilterLimits(params_.y_min, params_.y_max);
        pass.filter(*cloud);

        //3、移除上方的点 ，z轴为高度方向
        pass.setInputCloud(cloud);
        pass.setFilterFieldName("z");
        pass.setFilterLimits(params_.z_min, params_.z_max);
        pass.filter(*cloud);
        
    }

    void PointCloudProcessor::intensityBasedFilter(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud){

        //直接阈值过滤
        pcl::Indices indices;
        pcl::PassThrough<pcl::PointXYZI>pass;
        pass.setInputCloud(cloud);
        pass.setFilterFieldName("intensity");
        pass.setFilterLimits(params_.intensity_threshold, 120);
        pass.filter(indices);

        //直接操作原始点云
        pcl::PointCloud<pcl::PointXYZI>temp;
        pcl::copyPointCloud(*cloud, indices, temp);
        cloud->swap(temp);
    }

    void PointCloudProcessor::downsampleCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud){
        
        pcl::VoxelGrid<pcl::PointXYZI>voxel;
        voxel.setInputCloud(cloud);
        voxel.setLeafSize(params_.voxel_leaf_size, params_.voxel_leaf_size, params_.voxel_leaf_size);
        voxel.filter(*cloud);

    }

    void PointCloudProcessor::removeOutliers(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud){

        pcl::StatisticalOutlierRemoval<pcl::PointXYZI> sor;
        sor.setInputCloud(cloud);
        sor.setMeanK(params_.sor_mean_k);
        sor.setStddevMulThresh(params_.sor_std_mul);
        sor.filter(*cloud);

    }

    void PointCloudProcessor::accumulateCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud, 
                                            pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud){
        
        if(!input_cloud || input_cloud->empty()){
            return;
        } 

         //累积前先降采样当前帧
        downsampleCloud(input_cloud);

        //达到最大帧数时重置
        if (frame_counter_ >= params_.max_frames) {
        //pcl::PointCloud<pcl::PointXYZI> empty;  // 创建空点云
        //output_cloud->swap(empty);              // 交换内容
        output_cloud->clear();
        frame_counter_ = 0;
        }
       
        *output_cloud += *input_cloud;
        frame_counter_++;


    }

    bool PointCloudProcessor::isAccumulationComplete() const{
        return frame_counter_ >= params_.max_frames;
    }

    void PointCloudProcessor::resetAccumulation(){
        frame_counter_ = 0;
    }

} 
// namespace mult_lidar
